#include "GMatrix3x3.h"

#include "GMath.h"
#include <memory.h>

namespace GMath
{
	const GMatrix3x3 GMatrix3x3::Identity = GMatrix3x3(	1.0f, 0.0f, 0.0f,
														0.0f, 1.0f, 0.0f,
														0.0f, 0.0f, 1.0f );

	const GMatrix3x3 GMatrix3x3::Zero = GMatrix3x3(	0.0f, 0.0f, 0.0f,
													0.0f, 0.0f, 0.0f,
													0.0f, 0.0f, 0.0f );

	const GMatrix3x3 GMatrix3x3::One = GMatrix3x3(	1.0f, 1.0f, 1.0f,
													1.0f, 1.0f, 1.0f,
													1.0f, 1.0f, 1.0f );

	GMatrix3x3::GMatrix3x3()
	{
		I11 = 1.0f; I12 = 0.0f; I13 = 0.0f;
		I21 = 0.0f; I22 = 1.0f; I23 = 0.0f;
		I31 = 0.0f; I32 = 0.0f; I33 = 1.0f;
	}

	GMatrix3x3::GMatrix3x3(float M11, float M12, float M13,
						   float M21, float M22, float M23,
						   float M31, float M32, float M33)
	{
		I11 = M11; I12 = M12; I13 = M13;
		I21 = M21; I22 = M22; I23 = M23;
		I31 = M31; I32 = M32; I33 = M33;
	}

	GMatrix3x3::operator float* ()
	{
		return Arr;
	}

	GMatrix3x3::operator const float* () const
	{
		return (const float*)Arr;
	}

	float GMatrix3x3::Determinant() const
	{
		return I11 * (I22 * I33 - I32 * I23) -
			   I12 * (I21 * I33 - I31 * I23) + 
			   I13 * (I21 * I32 - I31 * I22);
	}

	bool GMatrix3x3::Inverse()
	{
		GMatrix3x3 Temp(*this);

		float Det = Determinant();

		if (EqualF(Det, 0.0f))
		{
			return false;
		}

		float InvDet = 1.0f / Det;

		I11 =  ( Temp.I22 * Temp.I33 - Temp.I23 * Temp.I32 ) * InvDet;
		I12 = -( Temp.I12 * Temp.I33 - Temp.I32 * Temp.I13 ) * InvDet;
		I13 =  ( Temp.I12 * Temp.I23 - Temp.I22 * Temp.I13 ) * InvDet;

		I21 = -( Temp.I21 * Temp.I33 - Temp.I23 * Temp.I31 ) * InvDet;
		I22 =  ( Temp.I11 * Temp.I33 - Temp.I31 * Temp.I13 ) * InvDet;
		I23 = -( Temp.I11 * Temp.I23 - Temp.I21 * Temp.I13 ) * InvDet;

		I31 =  ( Temp.I21 * Temp.I32 - Temp.I31 * Temp.I22 ) * InvDet;
		I32 = -( Temp.I11 * Temp.I32 - Temp.I31 * Temp.I12 ) * InvDet;
		I33 =  ( Temp.I11 * Temp.I22 - Temp.I12 * Temp.I21 ) * InvDet;

		return true;
	}

	bool GMatrix3x3::Inverse(OUTPUT GMatrix3x3& OutMatrix, const GMatrix3x3& Matrix)
	{
		float Det = Matrix.Determinant();

		if (EqualF(Det, 0.0f))
		{
			return false;
		}

		float InvDet = 1.0f / Det;

		OutMatrix.I11 =  ( Matrix.I22 * Matrix.I33 - Matrix.I23 * Matrix.I32 ) * InvDet;
		OutMatrix.I12 = -( Matrix.I12 * Matrix.I33 - Matrix.I32 * Matrix.I13 ) * InvDet;
		OutMatrix.I13 =  ( Matrix.I12 * Matrix.I23 - Matrix.I22 * Matrix.I13 ) * InvDet;

		OutMatrix.I21 = -( Matrix.I21 * Matrix.I33 - Matrix.I23 * Matrix.I31 ) * InvDet;
		OutMatrix.I22 =  ( Matrix.I11 * Matrix.I33 - Matrix.I31 * Matrix.I13 ) * InvDet;
		OutMatrix.I23 = -( Matrix.I11 * Matrix.I23 - Matrix.I21 * Matrix.I13 ) * InvDet;

		OutMatrix.I31 =  ( Matrix.I21 * Matrix.I32 - Matrix.I31 * Matrix.I22 ) * InvDet;
		OutMatrix.I32 = -( Matrix.I11 * Matrix.I32 - Matrix.I31 * Matrix.I12 ) * InvDet;
		OutMatrix.I33 =  ( Matrix.I11 * Matrix.I22 - Matrix.I12 * Matrix.I21 ) * InvDet;

		return true;
	}

	void GMatrix3x3::Transpose()
	{
		GMatrix3x3 Temp(*this);
		
		Arr[0]  = Temp.Arr[0];
		Arr[1]  = Temp.Arr[3];
		Arr[2]  = Temp.Arr[6];
		Arr[3]  = Temp.Arr[1];
		Arr[4]  = Temp.Arr[4];
		Arr[5]  = Temp.Arr[7];
		Arr[6]  = Temp.Arr[2];
		Arr[7]  = Temp.Arr[5];
		Arr[8]  = Temp.Arr[8];
	}

	void GMatrix3x3::Transpose(OUTPUT GMatrix3x3& OutMatrix, const GMatrix3x3& Matrix)
	{
		OutMatrix.Arr[0]  = Matrix.Arr[0];
		OutMatrix.Arr[1]  = Matrix.Arr[3];
		OutMatrix.Arr[2]  = Matrix.Arr[6];
		OutMatrix.Arr[3]  = Matrix.Arr[1];
		OutMatrix.Arr[4]  = Matrix.Arr[4];
		OutMatrix.Arr[5]  = Matrix.Arr[7];
		OutMatrix.Arr[6]  = Matrix.Arr[2];
		OutMatrix.Arr[7]  = Matrix.Arr[5];
		OutMatrix.Arr[8]  = Matrix.Arr[8];
	}

	void GMatrix3x3::Decompose(OUTPUT GVector2& OutScale, OUTPUT float& OutRotation, OUTPUT GVector2& OutTranslation, const GMatrix3x3& Matrix)
	{
		OutScale.x = SqrtF(Matrix.I11 * Matrix.I11 + Matrix.I12 * Matrix.I12);
		OutScale.y = SqrtF(Matrix.I21 * Matrix.I21 + Matrix.I22 * Matrix.I22);

		OutTranslation.x = Matrix.I31;
		OutTranslation.y = Matrix.I32;

		OutRotation = ArcCosF(Matrix.I11 / OutScale.x);
	}

	void GMatrix3x3::Multiply(OUTPUT GVector2& Output, const GVector2& Vector, const GMatrix3x3& Matrix)
	{
		Output = Vector * Matrix;
	}

	void GMatrix3x3::Multiply(OUTPUT GVector3& Output, const GVector3& Vector, const GMatrix3x3& Matrix)
	{
		Output = Vector * Matrix;
	}

	void GMatrix3x3::Multiply(OUTPUT GMatrix3x3& Output, const GMatrix3x3& MatrixA, const GMatrix3x3& MatrixB)
	{
		Output = MatrixA * MatrixB;
	}

	float GMatrix3x3::operator () (GUInt32 Row, GUInt32 Column)
	{
		return Mat[Row][Column];
	}
	
	float GMatrix3x3::operator () (GUInt32 Row, GUInt32 Column) const
	{
		return Mat[Row][Column];
	}

	GMatrix3x3& GMatrix3x3::operator = (const GMatrix3x3& Matrix)
	{
		memcpy(Arr, Matrix.Arr, sizeof(GMatrix3x3));
		return *this;
	}

	GMatrix3x3& GMatrix3x3::operator += (const GMatrix3x3& Matrix)
	{
		I11 += Matrix.I11; I12 += Matrix.I12; I13 += Matrix.I13;
		I21 += Matrix.I21; I22 += Matrix.I22; I23 += Matrix.I23;
		I31 += Matrix.I31; I32 += Matrix.I32; I33 += Matrix.I33;
		
		return *this;
	}

	GMatrix3x3& GMatrix3x3::operator -= (const GMatrix3x3& Matrix)
	{
		I11 -= Matrix.I11; I12 -= Matrix.I12; I13 -= Matrix.I13;
		I21 -= Matrix.I21; I22 -= Matrix.I22; I23 -= Matrix.I23;
		I31 -= Matrix.I31; I32 -= Matrix.I32; I33 -= Matrix.I33;
		return *this;
	}

	GMatrix3x3& GMatrix3x3::operator *= (const GMatrix3x3& Matrix)
	{
		GMatrix3x3 Temp(*this);
		
		for (int I = 0; I < 9; I += 3)
		{
			for (int J = 0; J < 3; J++)
			{
				Arr[I+J] =	Temp.Arr[I+0] * Matrix.Arr[J+0] + 
							Temp.Arr[I+1] * Matrix.Arr[J+3] + 
							Temp.Arr[I+2] * Matrix.Arr[J+6];
			}
		}

		return *this;
	}

	GMatrix3x3& GMatrix3x3::operator *= (const float Value)
	{
		I11 *= Value; I12 *= Value; I13 *= Value;
		I21 *= Value; I22 *= Value; I23 *= Value;
		I31 *= Value; I32 *= Value; I33 *= Value;
		return *this;
	}

	GMatrix3x3& GMatrix3x3::operator /= (const float Value)
	{
		float Inverse = 1.0f / Value;
		I11 *= Inverse; I12 *= Inverse; I13 *= Inverse;
		I21 *= Inverse; I22 *= Inverse; I23 *= Inverse;
		I31 *= Inverse; I32 *= Inverse; I33 *= Inverse;
		return *this;
	}

	GMatrix3x3 GMatrix3x3::operator + (const GMatrix3x3& Matrix) const
	{
		return GMatrix3x3(	I11 + Matrix.I11, I12 + Matrix.I12, I13 + Matrix.I13,
							I21 + Matrix.I21, I22 + Matrix.I22, I23 + Matrix.I23,
							I31 + Matrix.I31, I32 + Matrix.I32, I33 + Matrix.I33 );
	}

	GMatrix3x3 GMatrix3x3::operator - (const GMatrix3x3& Matrix) const
	{
		return GMatrix3x3(	I11 - Matrix.I11, I12 - Matrix.I12, I13 - Matrix.I13,
							I21 - Matrix.I21, I22 - Matrix.I22, I23 - Matrix.I23,
							I31 - Matrix.I31, I32 - Matrix.I32, I33 - Matrix.I33 );
	}

	GMatrix3x3 GMatrix3x3::operator * (const GMatrix3x3& Matrix) const
	{
		GMatrix3x3 TempMatrix;

		for (int I = 0; I < 9; I += 3)
		{
			for (int J = 0; J < 3; J++) 
			{
				TempMatrix.Arr[I+J] =	Arr[I+0] * Matrix.Arr[J+0] + 
										Arr[I+1] * Matrix.Arr[J+3] + 
										Arr[I+2] * Matrix.Arr[J+6];
			}
		}

		return TempMatrix;
	}

	GMatrix3x3 GMatrix3x3::operator * (const float Value) const
	{
		return GMatrix3x3(	I11 * Value, I12 * Value, I13 * Value,
							I21 * Value, I22 * Value, I23 * Value,
							I31 * Value, I32 * Value, I33 * Value );
	}

	GMatrix3x3 GMatrix3x3::operator / (const float Value) const
	{
		float Inverse = 1.0f / Value;
		return GMatrix3x3(	I11 * Inverse, I12 * Inverse, I13 * Inverse,
							I21 * Inverse, I22 * Inverse, I23 * Inverse,
							I31 * Inverse, I32 * Inverse, I33 * Inverse );
	}

	GMatrix3x3 GMatrix3x3::operator + () const
	{
		return *this;
	}

	GMatrix3x3 GMatrix3x3::operator - () const
	{
		return GMatrix3x3(	-I11, -I12, -I13,
							-I21, -I22, -I23,
							-I31, -I32, -I33 );
	}

	bool GMatrix3x3::operator == (const GMatrix3x3& Matrix) const
	{
		for (GUInt32 I = 0; I < 9; ++I)
		{
			if (EqualF(Arr[I], Matrix.Arr[I]))
				return false;
		}

		return true;
	}

	bool GMatrix3x3::operator != (const GMatrix3x3& Matrix) const
	{
		return !operator ==(Matrix);
	}

	GMatrix3x3 operator * (float Value, const GMatrix3x3& Matrix)
	{
		return GMatrix3x3(	Value * Matrix.I11, Value * Matrix.I12, Value * Matrix.I13,
							Value * Matrix.I21, Value * Matrix.I22, Value * Matrix.I23,
							Value * Matrix.I31, Value * Matrix.I32, Value * Matrix.I33 );
	}

	GVector3 operator * (const GVector3& Vector, const GMatrix3x3& Matrix)
	{
		GVector3 Output;
		Output.x = (Vector.x * Matrix.I11) + (Vector.y * Matrix.I21) + (Vector.z * Matrix.I31);
		Output.y = (Vector.x * Matrix.I12) + (Vector.y * Matrix.I22) + (Vector.z * Matrix.I32);
		Output.z = (Vector.x * Matrix.I13) + (Vector.y * Matrix.I23) + (Vector.z * Matrix.I33);
			
		return Output;
	}

	GVector2 operator * (const GVector2& Vector, const GMatrix3x3& Matrix)
	{
		GVector2 Output;
		Output.x = (Vector.x * Matrix.I11) + (Vector.y * Matrix.I21);
		Output.y = (Vector.x * Matrix.I12) + (Vector.y * Matrix.I22);
		return Output;
	}

}